//-----------------------------------------------------------------------------
//
// @file	Matrix.cpp
// @brief	Matrix  classes (code modified from DonHouse, Texas A&M Univ)
// @author	Vinod Melapudi
// @date	20Feb2010
//
//-----------------------------------------------------------------------------

#include <iomanip>
#include <iostream>
#include "Matrix.h"

using namespace std;


/* Matrix Descriptions and Operations */

// Matrix constructors
Matrix2x2::Matrix2x2(float a11, float a12,
					 float a21, float a22)
{
	set(a11, a12, a21, a22);
}

Matrix3x3::Matrix3x3(float a11, float a12,
					 float a21, float a22)
{
	set(a11, a12, a21, a22);
}

Matrix3x3::Matrix3x3(float a11, float a12, float a13,
					 float a21, float a22, float a23,
					 float a31, float a32, float a33)
{
	set(a11, a12, a13, a21, a22, a23, a31, a32, a33);
}

Matrix4x4::Matrix4x4(float a11, float a12, float a13,
					 float a21, float a22, float a23,
					 float a31, float a32, float a33)
{
	set(a11, a12, a13, a21, a22, a23, a31, a32, a33);
}

Matrix4x4::Matrix4x4(float a11, float a12, float a13, float a14,
					 float a21, float a22, float a23, float a24,
					 float a31, float a32, float a33, float a34,
					 float a41, float a42, float a43, float a44)
{
	set(a11, a12, a13, a14, a21, a22, a23, a24,
		a31, a32, a33, a34, a41, a42, a43, a44);
}

Matrix::Matrix(int rows, int cols, const float *M)
{
	int i, j, k;
	setsize(rows, cols);

	if(M != NULL)
		for(i = 0, k = 0; i < rows; i++)
			for(j = 0; j < cols; j++)
				(row[i])[j] = M[k++];
}

Matrix::Matrix(const Matrix& M)
{
	int i, j;

	setsize(M.Nrows, M.Ncols);
	for(i = 0; i < Nrows; i++)
		for(j = 0; j < Ncols; j++)
			row[i][j] = M.row[i][j];
}

Matrix::Matrix(float a11, float a12,
			   float a21, float a22)
{
	setsize(2, 2);
	row[0].set(a11, a12);
	row[1].set(a21, a22);
}

Matrix::Matrix(float a11, float a12, float a13,
			   float a21, float a22, float a23,
			   float a31, float a32, float a33)
{
	setsize(3, 3);
	row[0].set(a11, a12, a13);
	row[1].set(a21, a22, a23);
	row[2].set(a31, a32, a33);
}

Matrix::Matrix(float a11, float a12, float a13, float a14,
			   float a21, float a22, float a23, float a24,
			   float a31, float a32, float a33, float a34,
			   float a41, float a42, float a43, float a44)
{
	setsize(4, 4);
	row[0].set(a11, a12, a13, a14);
	row[1].set(a21, a22, a23, a24);
	row[2].set(a31, a32, a33, a34);
	row[3].set(a41, a42, a43, a44);
}

// Destructor
Matrix::~Matrix()
{
	delete []row;
}

void Matrix::setsize(int rows, int cols)
{
	int i;

	if(rows < 0 || cols < 0 || ((rows == 0 || cols == 0) && cols != rows)){
		cerr << "Matrix size impossible (negative or zero rows or cols)" << endl;
		exit(1);
	}
	Nrows = rows;
	Ncols = cols;

	if(Nrows == 0)
		row = NULL;
	else{
		row = new Vector[rows];
		if(!row){
			cerr << "not enough memory to allocate " << rows << " x " << cols << 
				" Matrix" << endl;
			exit(1);
		}
		for(i = 0; i < rows; i++)
			row[i].setsize(cols);
	}

}

int Matrix::nrows() const
{
	return Nrows;
}

int Matrix::ncols() const
{
	return Ncols;
}

//
// return row i of matrix as a vector
// Routines returning an lvalue: i.e. M[i] returns ref to row i
//
Vector2& Matrix2x2::operator[](int i)
{
	if(i < 0 || i > 1){
		cerr << "Matrix2x2 row index " << i << " out of bounds" << endl;
		exit(1);
	}

	return (row[i]);
}
Vector3& Matrix3x3::operator[](int i)
{
	if(i < 0 || i > 2){
		cerr << "Matrix3x3 row index " << i << " out of bounds" << endl;
		exit(1);
	}

	return (row[i]);
}
Vector4& Matrix4x4::operator[](int i)
{
	if(i < 0 || i > 3){
		cerr << "Matrix4x4 row index " << i << " out of bounds" << endl;
		exit(1);
	}

	return (row[i]);
}
Vector& Matrix::operator[](int i)
{
	if(i < 0 || i >= Nrows){
		cerr << "Matrix row index of " << i << " out of bounds" << endl;
		exit(1);
	}

	return (row[i]);
}

//
// return row i of matrix as a vector
// Routines returning an rvalue: i.e. X[i] returns const ref to row i
//
const Vector2& Matrix2x2::operator[](int i) const
{
	if(i < 0 || i > 1){
		cerr << "Matrix2x2 row index " << i << " out of bounds" << endl;
		exit(1);
	}

	return (row[i]);
}
const Vector3& Matrix3x3::operator[](int i) const
{
	if(i < 0 || i > 2){
		cerr << "Matrix3x3 row index " << i << " out of bounds" << endl;
		exit(1);
	}

	return (row[i]);
}
const Vector4& Matrix4x4::operator[](int i) const
{
	if(i < 0 || i > 3){
		cerr << "Matrix4x4 row index " << i << " out of bounds" << endl;
		exit(1);
	}

	return (row[i]);
}
const Vector& Matrix::operator[](int i) const
{
	if(i < 0 || i >= Nrows){
		cerr << "Matrix row index of " << i << " out of bounds" << endl;
		exit(1);
	}

	return (row[i]);
}

// Casts of Matrices into other forms of Matrices
Matrix2x2::operator Matrix3x3()
{
	Matrix3x3 m;
	int i, j;

	for(i = 0; i < 2; i++)
		for(j = 0; j < 2; j++)
			m[i][j] = row[i][j];

	m[2].z = 1.0;

	return m;
}

Matrix2x2::operator Matrix4x4()
{
	Matrix4x4 m;
	int i, j;

	for(i = 0; i < 2; i++)
		for(j = 0; j < 2; j++)
			m[i][j] = row[i][j];

	m[2].z = 1.0;
	m[3].w = 1.0;

	return m;
}

Matrix2x2::operator Matrix()
{
	Matrix m(2, 2);
	int i, j;

	for(i = 0; i < 2; i++)
		for(j = 0; j < 2; j++)
			m[i][j] = row[i][j];

	return m;
}

Matrix3x3::operator Matrix4x4()
{
	Matrix4x4 m;
	int i, j;

	for(i = 0; i < 3; i++)
		for(j = 0; j < 3; j++)
			m[i][j] = row[i][j];

	m[3].w = 1.0;

	return m;
}

Matrix3x3::operator Matrix()
{
	Matrix m(3, 3);
	int i, j;

	for(i = 0; i < 3; i++)
		for(j = 0; j < 3; j++)
			m[i][j] = row[i][j];

	return m;
}

Matrix4x4::operator Matrix()
{
	Matrix m(4, 4);
	int i, j;

	for(i = 0; i < 4; i++)
		for(j = 0; j < 4; j++)
			m[i][j] = row[i][j];

	return m;
}

Matrix::operator Matrix2x2()
{
	if(Nrows != 2 || Ncols != 2){
		cerr << "cannot cast " << Nrows << " x " << Ncols << 
			" Matrix to Matrix2x2" << endl;
		exit(1);
	}

	Matrix2x2 m;
	int i, j;

	for(i = 0; i < 2; i++)
		for(j = 0; j < 2; j++)
			m[i][j] = row[i][j];

	return m;  
}

Matrix::operator Matrix3x3()
{
	if(Nrows != 3 || Ncols != 3){
		cerr << "cannot cast " << Nrows << " x " << Ncols << 
			" Matrix to Matrix3x3" << endl;
		exit(1);
	}

	Matrix3x3 m;
	int i, j;

	for(i = 0; i < 3; i++)
		for(j = 0; j < 3; j++)
			m[i][j] = row[i][j];

	return m;  
}

Matrix::operator Matrix4x4()
{
	if(Nrows != 4 || Ncols != 4){
		cerr << "cannot cast " << Nrows << " x " << Ncols << 
			" Matrix to Matrix4x4" << endl;
		exit(1);
	}

	Matrix4x4 m;
	int i, j;

	for(i = 0; i < 4; i++)
		for(j = 0; j < 4; j++)
			m[i][j] = row[i][j];

	return m;  
}

void Matrix2x2::print(int w, int p) const
{
	int i;

	for(i = 0; i < 2; i++){
		cout << setw(w) << setprecision(p) << round(row[i].x, p) << " ";
		cout << setw(w) << setprecision(p) << round(row[i].y, p);
		cout << endl;
	}
}

void Matrix3x3::print(int w, int p) const
{
	int i;

	for(i = 0; i < 3; i++){
		cout << setw(w) << setprecision(p) << round(row[i].x, p) << " ";
		cout << setw(w) << setprecision(p) << round(row[i].y, p) << " ";
		cout << setw(w) << setprecision(p) << round(row[i].z, p);
		cout << endl;
	}
}

void Matrix4x4::print(int w, int p) const
{
	int i;

	for(i = 0; i < 4; i++){
		cout << setw(w) << setprecision(p) << round(row[i].x, p) << " ";
		cout << setw(w) << setprecision(p) << round(row[i].y, p) << " ";
		cout << setw(w) << setprecision(p) << round(row[i].z, p) << " ";
		cout << setw(w) << setprecision(p) << round(row[i].w, p);
		cout << endl;
	}
}

void Matrix::print(int w, int p) const
{
	int i, j;

	for(i = 0; i < Nrows; i++){
		for(j = 0; j < Ncols; j++)
			cout << setw(w) << setprecision(p) << round(row[i][j], p) << " ";
		cout << endl;
	}
}

void Matrix2x2::set(float a11, float a12,
					float a21, float a22)
{
	row[0].set(a11, a12);
	row[1].set(a21, a22);
}

void Matrix3x3::set(float a11, float a12,
					float a21, float a22)
{
	row[0].set(a11, a12, 0);
	row[1].set(a21, a22, 0);
	row[2].set(0, 0, 1);
}

void Matrix3x3::set(float a11, float a12, float a13,
					float a21, float a22, float a23,
					float a31, float a32, float a33)
{
	row[0].set(a11, a12, a13);
	row[1].set(a21, a22, a23);
	row[2].set(a31, a32, a33);
}

void Matrix4x4::set(float a11, float a12, float a13,
					float a21, float a22, float a23,
					float a31, float a32, float a33)
{
	row[0].set(a11, a12, a13, 0);
	row[1].set(a21, a22, a23, 0);
	row[2].set(a31, a32, a33, 0);
	row[3].set(0, 0, 0, 1);
}

void Matrix4x4::set(float a11, float a12, float a13, float a14,
					float a21, float a22, float a23, float a24,
					float a31, float a32, float a33, float a34,
					float a41, float a42, float a43, float a44)
{
	row[0].set(a11, a12, a13, a14);
	row[1].set(a21, a22, a23, a24);
	row[2].set(a31, a32, a33, a34);
	row[3].set(a41, a42, a43, a44);
}

void Matrix::set(float *M)
{
	int i, j;

	for(i = 0; i < Nrows; i++)
		for(j = 0; j < Ncols; j++)
			row[i][j] = M[i * Ncols + j];
}

void Matrix::set(float a11, float a12,
				 float a21, float a22)
{
	if(Nrows != 2 || Ncols != 2){
		cerr << "2 x 2 set of " << Nrows << " x " << Ncols << " Matrix" << endl;
		exit(1);
	}
	row[0].set(a11, a12);
	row[1].set(a21, a22);
}

void Matrix::set(float a11, float a12, float a13,
				 float a21, float a22, float a23,
				 float a31, float a32, float a33)
{
	if(Nrows != 3 || Ncols != 3){
		cerr << "3 x 3 set of " << Nrows << " x " << Ncols << 
			" Matrix" << endl;
		exit(1);
	}
	row[0].set(a11, a12, a13);
	row[1].set(a21, a22, a23);
	row[2].set(a31, a32, a33);
}

void Matrix::set(float a11, float a12, float a13, float a14,
				 float a21, float a22, float a23, float a24,
				 float a31, float a32, float a33, float a34,
				 float a41, float a42, float a43, float a44)
{
	if(Nrows != 4 || Ncols != 4){
		cerr << "4 x 4 set of " << Nrows << " x " << Ncols << " Matrix" << endl;
		exit(1);
	}
	row[0].set(a11, a12, a13, a14);
	row[1].set(a21, a22, a23, a24);
	row[2].set(a31, a32, a33, a34);
	row[3].set(a41, a42, a43, a44);
}

void Matrix2x2::identity()
{
	set(1, 0, 0, 1);
}

void Matrix3x3::identity()
{
	set(1, 0, 0, 0, 1, 0, 0, 0, 1);
}

void Matrix4x4::identity()
{
	set(1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1);
}

void Matrix::identity()
{
	int i, j;

	for(i = 0; i < Nrows; i++)
		for(j = 0; j < Ncols; j++)
			if(i == j)
				row[i][j] = 1;
			else
				row[i][j] = 0;
}

Matrix2x2 Matrix2x2::transpose() const
{
	Matrix2x2 transM;

	transM[0].x = row[0].x;
	transM[1].x = row[0].y;
	transM[0].y = row[1].x;
	transM[1].y = row[1].y;

	return transM;
}

Matrix3x3 Matrix3x3::transpose() const
{
	Matrix3x3 transM;

	transM[0].x = row[0].x;
	transM[1].x = row[0].y;
	transM[2].x = row[0].z;
	transM[0].y = row[1].x;
	transM[1].y = row[1].y;
	transM[2].y = row[1].z;
	transM[0].z = row[2].x;
	transM[1].z = row[2].y;
	transM[2].z = row[2].z;

	return transM;
}

Matrix4x4 Matrix4x4::transpose() const
{
	Matrix4x4 transM;

	transM[0].x = row[0].x;
	transM[1].x = row[0].y;
	transM[2].x = row[0].z;
	transM[3].x = row[0].w;
	transM[0].y = row[1].x;
	transM[1].y = row[1].y;
	transM[2].y = row[1].z;
	transM[3].y = row[1].w;
	transM[0].z = row[2].x;
	transM[1].z = row[2].y;
	transM[2].z = row[2].z;
	transM[3].z = row[2].w;
	transM[0].w = row[3].x;
	transM[1].w = row[3].y;
	transM[2].w = row[3].z;
	transM[3].w = row[3].w;

	return transM;
}

Matrix Matrix::transpose() const
{
	Matrix transM(Ncols, Nrows);
	int i, j;

	for(i = 0; i < Nrows; i++)
		for(j = 0; j < Ncols; j++)
			transM[j][i] = row[i][j];

	return transM;
}

//////////////////////////////////////////////////////////////////////////
// the following matrix operations are used to find the inverse of an 
// NxN matrix. Adapted from Numerical Recipes by (Frank) Sebastian Grassia
//////////////////////////////////////////////////////////////////////////

Matrix2x2 Matrix2x2::inv() const
{
	Matrix2x2 invM;
	float d;

	d = row[0].x*row[1].y - row[0].y*row[1].x;

	if(d == 0.0)
		cerr << "inverse of singular Matrix2x2" << endl;

	invM[0].x = row[1].y / d;
	invM[0].y = -row[0].y / d;
	invM[1].x = -row[1].x / d;
	invM[1].y = row[0].x / d;

	return invM;
}

Matrix3x3 Matrix3x3::inv() const
{
	Matrix3x3 invM;
	float d;

	d = row[0].x*row[1].y*row[2].z + row[0].y*row[1].z*row[2].x +
		row[0].z*row[2].y*row[1].x - row[0].z*row[1].y*row[2].x - 
		row[0].y*row[1].x*row[2].z - row[0].x*row[2].y*row[1].z;

	if(d == 0.0)
		cerr << "inverse of singular Matrix3x3" << endl;

	invM[0].x = (row[1].y*row[2].z - row[1].z*row[2].y) / d;
	invM[0].y = (row[0].z*row[2].y - row[0].y*row[2].z) / d;
	invM[0].z = (row[0].y*row[1].z - row[0].z*row[1].y) / d;
	invM[1].x = (row[1].z*row[2].x - row[1].x*row[2].z) / d;
	invM[1].y = (row[0].x*row[2].z - row[0].z*row[2].x) / d;
	invM[1].z = (row[0].z*row[1].x - row[0].x*row[1].z) / d;
	invM[2].x = (row[1].x*row[2].y - row[1].y*row[2].x) / d;
	invM[2].y = (row[0].y*row[2].x - row[0].x*row[2].y) / d;
	invM[2].z = (row[0].x*row[1].y - row[0].y*row[1].x) / d;

	return invM;
}

Matrix4x4 LU_Decompose(const Matrix4x4& M, int *indx)
{
	int  i, imax, j, k;
	float big, dum, sum, temp;
	float vv[4];
	Matrix4x4 LU_M = M;
	int N = 4;

	for(i = 0; i < N; i++){
		big = 0.0;
		for(j = 0; j < N; j++)
			if((temp = fabs(M.row[i][j])) > big)
				big = temp;
		if(big == 0.0)
			cerr << "inverse of singular Matrix4x4" << endl;

		vv[i] = 1.0 / big;
	}

	for(j = 0; j < N; j++){
		for(i = 0; i < j; i++){
			sum = LU_M[i][j];
			for(k = 0; k < i; k++)
				sum -= LU_M[i][k] * LU_M[k][j];
			LU_M[i][j] = sum;
		}
		big = 0.0;
		for(i = j; i < N; i++){
			sum = LU_M[i][j];
			for(k = 0; k < j; k++)
				sum -= LU_M[i][k] * LU_M[k][j];
			LU_M[i][j] = sum;
			if((dum = vv[i]*fabs(sum)) >= big){
				big = dum;
				imax = i;
			}
		}
		if(j != imax){
			for(k = 0; k < N; k++){
				dum = LU_M[imax][k];
				LU_M[imax][k] = LU_M[j][k];
				LU_M[j][k] = dum;
			}
			vv[imax] = vv[j];
		}
		indx[j] = imax;
		if(j < N - 1){
			dum = 1.0 / LU_M[j][j];
			for(i = j + 1; i < N; i++)
				LU_M[i][j] *= dum;
		}
	}

	return LU_M;
}

void LU_back_substitution(const Matrix4x4& M, int *indx, float col[])
{
	int i, ii = -1, ip,j;
	float sum;
	int N = 4;

	for(i = 0; i < N; i++){
		ip = indx[i];
		sum = col[ip];
		col[ip] = col[i];
		if(ii >= 0)
			for(j = ii; j < i; j++)
				sum -= M.row[i][j] * col[j];
		else if(sum)
			ii = i;
		col[i] = sum;
	}

	for(i = N - 1; i >= 0; i--){
		sum = col[i];
		for(j = i + 1; j < N; j++)
			sum -= M.row[i][j] * col[j];
		col[i] = sum / M.row[i][i];
	}
}

Matrix4x4 Matrix4x4::inv() const
{
	Matrix4x4 LU_M, invM;
	int i, j, indx[4];
	float col[4];

	LU_M = LU_Decompose(*this, indx);

	for(j = 0; j < 4; j++){
		for(i = 0; i < 4; i++)
			col[i] = 0.0;
		col[j] = 1.0;
		LU_back_substitution(LU_M, indx, col);
		for(i = 0; i < 4; i++)
			invM[i][j] = col[i];
	}

	return invM;
}

Matrix LU_Decompose(const Matrix& M, int *indx)
{
	int  i, imax, j, k;
	float big, dum, sum, temp;
	float *vv = new float[M.Nrows];
	Matrix LU_M(M);
	int N = M.Nrows;

	for(i = 0; i < N; i++){
		big = 0.0;
		for(j = 0; j < N; j++)
			if((temp = fabs(M[i][j])) > big)
				big = temp;
		if(big == 0.0)
			cerr << "inverse of singular " << M.Nrows << " x " << M.Ncols << 
			" Matrix" << endl;

		vv[i] = 1.0 / big;
	}

	for(j = 0; j < N; j++){
		for(i = 0; i < j; i++){
			sum = LU_M[i][j];
			for(k = 0; k < i; k++)
				sum -= LU_M[i][k] * LU_M[k][j];
			LU_M[i][j] = sum;
		}
		big = 0.0;
		for(i = j; i < N; i++){
			sum = LU_M[i][j];
			for(k = 0; k < j; k++)
				sum -= LU_M[i][k] * LU_M[k][j];
			LU_M[i][j] = sum;
			if((dum = vv[i]*fabs(sum)) >= big){
				big = dum;
				imax = i;
			}
		}
		if(j != imax){
			for(k = 0; k < N; k++){
				dum = LU_M[imax][k];
				LU_M[imax][k] = LU_M[j][k];
				LU_M[j][k] = dum;
			}
			vv[imax] = vv[j];
		}
		indx[j] = imax;
		if(j < N - 1){
			dum = 1.0 / LU_M[j][j];
			for(i = j + 1; i < N; i++)
				LU_M[i][j] *= dum;
		}
	}

	delete vv;
	return LU_M;
}

void LU_back_substitution(const Matrix& M, int *indx, float col[])
{
	int i, ii = -1, ip,j;
	float sum;
	int N = M.Nrows;

	for(i = 0; i < N; i++){
		ip = indx[i];
		sum = col[ip];
		col[ip] = col[i];
		if(ii >= 0)
			for(j = ii; j < i; j++)
				sum -= M.row[i][j] * col[j];
		else if(sum)
			ii = i;
		col[i] = sum;
	}

	for(i = N - 1; i >= 0; i--){
		sum = col[i];
		for(j = i + 1; j < N; j++)
			sum -= M.row[i][j] * col[j];
		col[i] = sum / M.row[i][i];
	}
}

Matrix Matrix::inv() const
{
	if(Nrows != Ncols){
		cerr << "cannot invert non-square " << Nrows << " x " << Ncols << 
			" Matrix" << endl;
		exit(1);
	}

	Matrix LU_M(Nrows, Nrows);
	Matrix invM(Nrows, Nrows);
	int i, j;
	int *indx = new int[Nrows];
	float *col = new float[Nrows];

	LU_M = LU_Decompose(*this, indx);

	for(j = 0; j < Nrows; j++){
		for(i = 0; i < Nrows; i++)
			col[i] = 0.0;
		col[j] = 1.0;
		LU_back_substitution(LU_M, indx, col);
		for(i = 0; i < Nrows; i++)
			invM[i][j] = col[i];
	}

	delete indx;
	delete col;
	return invM;
}

//
// Singular Value Decomposition of a Matrix: M = U W V^  (V^ is V transpose)
//
// U orthogonal matrix whose columns, for which wi != 0, span range of M
// W diagonal matrix of singular values (wi = 0 denotes a singularity)
// V orthogonal matrix whose columns, for which wi = 0, span null space of M
// M is (m x n), U is (m x n), W is (n x n), V is (n x n)
//
// U and V updated and returned as matrices
// W is updated and returned as a vector, since W is diagonal
//
/*
void Matrix::svd(Matrix &U, Vector &W, Matrix &V) const
{
int flag, i, its, j, jj, k, l, nm;
float anorm, c, f, g, h, s, scale, x, y, z;

int m = nrows();
int n = ncols();

// copy this matrix to U
U = *this;

// make sure that the other output matrices are the right size
W.setsize(n);
V.setsize(n, n);

// create working vector
Vector rv1(n);

// Householder reduction to bidiagonal form
g = scale = anorm = 0;

for(i = 0; i < n; i++){
l = i + 1;
rv1[i] = scale * g;
g = s = scale = 0;

if(i < m){
for(k = i; k < m; k++)
scale += fabs(U[k][i]);
if(scale != 0){
for(k = i; k < m; k++){
U[k][i] /= scale;
s += sqr(U[k][i]);
}
f = U[i][i];
g = -applysign(sqrt(s), f);
h = f * g - s;
U[i][i] = f - g;
for(j = l; j < n; j++){
for(s = 0, k = i; k < m; k++)
s += U[k][i] * U[k][j];
f = s / h;
for(k = i; k < m; k++)
U[k][j] += f * U[k][i];
}
for(k = i; k < m; k++)
U[k][i] = U[k][i] * scale;
}
}

W[i] = scale * g;
g = s = scale = 0;

if(i < m && i != (n - 1)){
for(k = l; k < n; k++)
scale += fabs(U[i][k]);
if(scale != 0){
for(k = l; k < n; k++){
U[i][k] /= scale;
s += sqr(U[i][k]);
}
f = U[i][l];
g = -applysign(sqrt(s), f);
h = f * g - s;
U[i][l] = f - g;
for(k = l; k < n; k++)
rv1[k] = U[i][k] / h;
for(j = l; j < m; j++){
for(s = 0, k = l; k < n; k++)
s += U[j][k] * U[i][k];
for(k = l; k < n; k++)
U[j][k] += s * rv1[k];
}
for(k = l; k < n; k++)
U[i][k] *= scale;
}
}
anorm = max(anorm, (fabs(W[i]) + fabs(rv1[i])));
}

// accumulation of hight-hand transformation
for(i = n - 1; i >= 0; i--){
if(i < n - 1){
if(g != 0){
for(j = l; j < n; j++)
V[j][i] = (U[i][j] / U[i][l]) / g;
for(j = l; j < n; j++){
for(s = 0, k = l; k < n; k++)
s += U[i][k] * V[k][j];
for(k = l; k < n; k++)
V[k][j] += s * V[k][i];
}
}
for(j = l; j < n; j++)
V[i][j] = V[j][i] = 0;
}
V[i][i] = 1;
g = rv1[i];
l = i;
}

// accumulation of left-hand transformation
for(i = min(m, n) - 1; i >= 0; i--){
l = i + 1;
g = W[i];
for(j = l; j < n; j++)
U[i][j] = 0;
if(g != 0){
g = 1 / g;
for(j = l; j < n; j++){
for(s = 0, k = l; k < m; k++)
s += U[k][i] * U[k][j];
f = (s / U[i][i]) * g;
for(k = i; k < m; k++)
U[k][j] = U[k][j] + f * U[k][i];
}
for(j = i; j < m; j++)
U[j][i] *= g;
}
else
for(j = i; j < m; j++)
U[j][i] = 0;
U[i][i]++;
}

// diagonalization of the bidiagonal form: loop over singular values,
// and over allowed iterations
for(k = n - 1; k >= 0; k--){
for(its = 1; its <= 30; its++){
flag = 1;
for(l = k; l >= 0; l--){		// test for splitting
nm = l - 1;			// note that rv1[0] is always zero
if(float(fabs(rv1[l]) + anorm) == anorm){
flag = 0;
break;
}
if(float(fabs(W[nm]) + anorm) == anorm)
break;
}
if(flag){
c =  0;
s = 1;
for(i = l; i <= k; i++){
f = s * rv1[i];
rv1[i] = c * rv1[i];
if(float(fabs(f) + anorm) == anorm)
break;
g = W[i];
h = pythag(f, g);
W[i] = h;
h = 1 / h;
c = g * h;
s = -f * h;
for(j = 0; j < m; j++){
y = U[j][nm];
z = U[j][i];
U[j][nm] = y * c + z * s;
U[j][i] = z * c - y * s;
}
}
}
z = W[k];

if(l == k){		// convergence
if(z < 0){		// singular value is made nonnegative
W[k] = -z;
for(j = 0; j < n; j++)
V[j][k] = - V[j][k];
}
break;
}

if(its == 30)
abort("no convergence in 30 svdcmp iterations");

x = W[l];
nm = k - 1;
y = W[nm];
g = rv1[nm];
h = rv1[k];
f = ((y - z) * ( y + z) + (g - h) * (g + h)) / (2 * h * y);
g = pythag(f, 1);
f = ((x- z) * (x + z) + h * ((y / (f + applysign(g, f))) - h)) / x;
c = s = 1;
for(j = l; j <= nm; j++){
i = j + 1;
g = rv1[i];
y = W[i];
h = s * g;
g = c * g;
z = pythag(f, h);
rv1[j] = z;
c = f / z;
s = h / z;
f = x * c + g * s;
g = g * c - x * s;
h = y * s;
y *= c;
for(jj = 0; jj < n; jj++){
x = V[jj][j];
z = V[jj][i];
V[jj][j] = x * c + z * s;
V[jj][i] = z * c - x * s;
}
z = pythag(f, h);

W[j] = z;		// rotation can be arbitrary if z = 0

if(z != 0){
z = 1 / z;
c = f * z;
s = h * z;
}

f = c * g + s * y;
x = c * y - s * g;

for(jj = 0; jj < m; jj++){
y = U[jj][j];
z = U[jj][i];
U[jj][j] = y * c + z * s;
U[jj][i] = z * c - y * s;
}
}
rv1[l] = 0;
rv1[k] = f;
W[k] = x;
}
}
}
*/
// returns a diagonal matrix with diagonal elements from the input vector
// note: this is not a friend of Matrix class rather than a method of Vector 
// class, since we wanted to maintain the independence of Vector from Matrix
Matrix diag(const Vector &V)
{
	int i;
	int n = V.getn();
	Matrix d(n, n);

	for(i = 0; i < n; i++)
		d[i][i] = V[i];

	return d;
}

/* Matrix op Matrix operations */

Matrix2x2 operator+(const Matrix2x2& m1, const Matrix2x2& m2)
{
	int i;
	Matrix2x2 result;

	for(i = 0; i < 2; i++){
		result.row[i].x = m1.row[i].x + m2.row[i].x;
		result.row[i].y = m1.row[i].y + m2.row[i].y;
	}

	return result;
}

Matrix2x2 operator-(const Matrix2x2& m1, const Matrix2x2& m2)
{
	int i;
	Matrix2x2 result;

	for(i = 0; i < 2; i++){
		result.row[i].x = m1.row[i].x - m2.row[i].x;
		result.row[i].y = m1.row[i].y - m2.row[i].y;
	}

	return result;
}

Matrix2x2 operator*(float a, const Matrix2x2& m)
{
	int i;
	Matrix2x2 result;

	for(i = 0; i < 2; i++){
		result.row[i].x = a * m.row[i].x;
		result.row[i].y = a * m.row[i].y;
	}

	return result;
}

Matrix2x2 Matrix2x2::operator*(float a) const
{
	int i;
	Matrix2x2 result;

	for(i = 0; i < 2; i++){
		result.row[i].x = a * row[i].x;
		result.row[i].y = a * row[i].y;
	}

	return result;
}

Matrix2x2 operator*(const Matrix2x2& m1, const Matrix2x2& m2)
{
	int i, j, rc;
	Matrix2x2 result;

	for(i = 0; i < 2; i++)
		for(j = 0; j < 2; j++){
			result.row[i][j] = 0;
			for(rc = 0; rc < 2; rc++)
				result.row[i][j] += m1.row[i][rc] * m2.row[rc][j];
		}

		return result;
}

Matrix3x3 operator+(const Matrix3x3& m1, const Matrix3x3& m2)
{
	int i, j;
	Matrix3x3 result;

	for(i = 0; i < 3; i++)
		for(j = 0; j < 3; j++)
			result.row[i][j] = m1.row[i][j] + m2.row[i][j];

	return result;
}

Matrix3x3 operator-(const Matrix3x3& m1, const Matrix3x3& m2)
{
	int i, j;
	Matrix3x3 result;

	for(i = 0; i < 3; i++)
		for(j = 0; j < 3; j++)
			result.row[i][j] = m1.row[i][j] - m2.row[i][j];

	return result;
}

Matrix3x3 operator*(float a, const Matrix3x3& m)
{
	Matrix3x3 result;
	int i, j;

	for(i = 0; i < 3; i++)
		for(j = 0; j < 3; j++)
			result.row[i][j] = a * m.row[i][j];

	return result;
}

Matrix3x3 Matrix3x3::operator*(float a) const
{
	Matrix3x3 result;
	int i, j;

	for(i = 0; i < 3; i++)
		for(j = 0; j < 3; j++)
			result[i][j] = a * row[i][j];

	return result;
}

Matrix3x3 operator*(const Matrix3x3& m1, const Matrix3x3& m2)
{
	int i, j, rc;
	Matrix3x3 result;

	for(i = 0; i < 3; i++)
		for(j = 0; j < 3; j++){
			result.row[i][j] = 0;
			for(rc = 0; rc < 3; rc++)
				result.row[i][j] += m1.row[i][rc] * m2[rc][j];
		}

		return result;
}

Matrix4x4 operator+(const Matrix4x4& m1, const Matrix4x4& m2)
{
	int i, j;
	Matrix4x4 result;

	for(i = 0; i < 4; i++)
		for(j = 0; j < 4; j++)
			result.row[i][j] = m1.row[i][j] + m2.row[i][j];

	return result;
}

Matrix4x4 operator-(const Matrix4x4& m1, const Matrix4x4& m2)
{
	int i, j;
	Matrix4x4 result;

	for(i = 0; i < 4; i++)
		for(j = 0; j < 4; j++)
			result.row[i][j] = m1.row[i][j] - m2.row[i][j];

	return result;
}

Matrix4x4 operator*(float a, const Matrix4x4& m)
{
	Matrix4x4 result;

	int i, j;
	for(i = 0; i < 4; i++)
		for(j = 0; j < 4; j++)
			result.row[i][j] = a * m.row[i][j];

	return result;
}

Matrix4x4 Matrix4x4::operator*(float a) const
{
	Matrix4x4 result;

	int i, j;
	for(i = 0; i < 4; i++)
		for(j = 0; j < 4; j++)
			result[i][j] = a * row[i][j];

	return result;
}

Matrix4x4 operator*(const Matrix4x4& m1, const Matrix4x4& m2)
{
	int i, j, rc;
	Matrix4x4 result;

	for(i = 0; i < 4; i++)
		for(j = 0; j < 4; j++){
			result.row[i][j] = 0;
			for(rc = 0; rc < 4; rc++)
				result.row[i][j] += m1.row[i][rc] * m2[rc][j];
		}

		return result;
}

Matrix operator+(const Matrix& m1, const Matrix& m2)
{
	if(m1.Nrows != m2.Nrows || m1.Ncols != m2.Ncols){
		cerr << "cannot add " << m1.Nrows << " x " << m1.Ncols << 
			" Matrix to " << m2.Nrows << " x " << m2.Ncols << " Matrix" << endl;
		exit(1);
	}
	Matrix result(m1.Nrows, m1.Ncols);

	int i, j;
	for(i = 0; i < m1.Nrows; i++)
		for(j = 0; j < m1.Ncols; j++)
			result.row[i][j] = m1.row[i][j] + m2.row[i][j];

	return result;
}

Matrix operator-(const Matrix& m1, const Matrix& m2)
{
	if(m1.Nrows != m2.Nrows || m1.Ncols != m2.Ncols){
		cerr << "cannot subtract " << m2.Nrows << " x " << m2.Ncols << " Matrix from " << m1.Nrows << " x " << m1.Ncols << " Matrix" << endl;
		exit(1);
	}
	Matrix result(m1.Nrows, m1.Ncols);

	int i, j;
	for(i = 0; i < m1.Nrows; i++)
		for(j = 0; j < m1.Ncols; j++)
			result.row[i][j] = m1.row[i][j] - m2.row[i][j];

	return result;
}

Matrix operator*(float a, const Matrix& m)
{
	Matrix result(m.Nrows, m.Ncols);

	int i, j;
	for(i = 0; i < m.Nrows; i++)
		for(j = 0; j < m.Ncols; j++)
			result.row[i][j] = a * m.row[i][j];

	return result;
}

Matrix Matrix::operator*(float a) const
{
	Matrix result(Nrows, Ncols);

	int i, j;
	for(i = 0; i < Nrows; i++)
		for(j = 0; j < Ncols; j++)
			result[i][j] = a * row[i][j];

	return result;
}

Matrix operator*(const Matrix& m1, const Matrix& m2)
{
	if(m1.Ncols != m2.Nrows){
		cerr << "cannot multiply " << m2.Nrows << " x " << m2.Ncols << 
			" Matrix by " << m1.Nrows << " x " << m1.Ncols << " Matrix" << endl;
		exit(1);
	}
	Matrix result(m1.Nrows, m2.Ncols);

	int i, j, rc;
	for(i = 0; i < m1.Nrows; i++)
		for(j = 0; j < m2.Ncols; j++){
			result.row[i][j] = 0;
			for(rc = 0; rc < m1.Ncols; rc++)
				result.row[i][j] += m1.row[i][rc] * m2[rc][j];
		}

		return result;
}

// Assignment
const Matrix& Matrix::operator=(const Matrix& m2)
{
	int i, j;

	if(Nrows != m2.Nrows || Ncols != m2.Ncols){
		delete []row;
		setsize(m2.Nrows, m2.Ncols);
	}

	for(i = 0; i < Nrows; i++)
		for(j = 0; j < Ncols; j++)
			(row[i])[j] = (m2.row[i])[j];

	return (*this);
}

/* Matrix-Vector Operations */

Vector2 operator*(const Matrix2x2& m, const Vector2& v)
{
	int i, j;
	float sum;
	Vector2 result;

	for(i = 0; i < 2; i++){
		sum = 0;
		for(j = 0; j < 2; j++)
			sum += m.row[i][j] * v[j];
		result[i] = sum;
	}

	return result;
}

Vector3 operator*(const Matrix3x3& m, const Vector3& v)
{
	int i, j;
	float sum;
	Vector3 result;

	for(i = 0; i < 3; i++){
		sum = 0;
		for(j = 0; j < 3; j++)
			sum += m.row[i][j] * v[j];
		result[i] = sum;
	}

	return result;
}

Vector4 operator*(const Matrix4x4& m, const Vector4& v)
{
	int i, j;
	float sum;
	Vector4 result;

	for(i = 0; i < 4; i++){
		sum = 0;
		for(j = 0; j < 4; j++)
			sum += m.row[i][j] * v[j];
		result[i] = sum;
	}

	return result;
}

// mat times vector
Vector3 operator*(const Matrix4x4& m, const Vector3& v)
{
	Vector4 v4(v);
	Vector4 v4t = m * v4;
	return Vector3(v4t.x, v4t.y, v4t.z);
}

// vector times mat
Vector3 operator*(const Vector3& v, const Matrix4x4& m)
{
	Vector4 v4(v);
	Vector4 v4t = v4 * m;
	return Vector3(v4t.x, v4t.y, v4t.z);
}

Vector operator*(const Matrix& M, const Vector& V)
{
	if(M.Ncols != V.getn()){
		cerr << "multiply of " << M.Nrows << " x " << M.Ncols << 
			" Matrix by " << V.getn() << " Vector" << endl;
		exit(1);
	}

	int i, j;
	float sum;
	Vector result(M.Nrows);

	for(i = 0; i < M.Nrows; i++){
		sum = 0;
		for(j = 0; j < M.Ncols; j++)
			sum += M[i][j] * V[j];
		result[i] = sum;
	}

	return result;
}

Vector2 operator*(const Vector2& v, const Matrix2x2& m)
{
	int i, j;
	float sum;
	Vector2 result;

	for(j = 0; j < 2; j++){
		sum = 0;
		for(i = 0; i < 2; i++)
			sum += v[i] * m.row[i][j];
		result[j] = sum;
	}

	return result;
}

Vector3 operator*(const Vector3& v, const Matrix3x3& m)
{
	int i, j;
	float sum;
	Vector3 result;

	for(j = 0; j < 3; j++){
		sum = 0;
		for(i = 0; i < 3; i++)
			sum += v[i] * m.row[i][j];
		result[j] = sum;
	}

	return result;
}

Vector4 operator*(const Vector4& v, const Matrix4x4& m)
{
	int i, j;
	float sum;
	Vector4 result;

	for(j = 0; j < 4; j++){
		sum = 0;
		for(i = 0; i < 4; i++)
			sum += v[i] * m.row[i][j];
		result[j] = sum;
	}

	return result;
}

Vector operator*(const Vector& V, const Matrix& M)
{
	if(M.Nrows != V.getn()){
		cerr << "multiply of " << V.getn() << " Vector by " << M.Nrows << 
			" x " << M.Ncols << " Matrix" << endl;
		exit(1);
	}

	int i, j;
	float sum;
	Vector result(M.Nrows);

	for(j = 0; j < M.Ncols; j++){
		sum = 0;
		for(i = 0; i < M.Nrows; i++)
			sum += M[i][j] * V[i];
		result[i] = sum;
	}

	return result;
}

// Outer product of v1, v2 (i.e. v1 times v2 transpose)
Matrix2x2 operator&(const Vector2& v1, const Vector2& v2)
{
	int i, j;
	Matrix2x2 product;

	for(i = 0; i < 2; i++)
		for(j = 0; j < 2; j++)
			product.row[i][j] = v1[i] * v2[j];

	return product;
}
Matrix3x3 operator&(const Vector3& v1, const Vector3& v2)
{
	int i, j;
	Matrix3x3 product;

	for(i = 0; i < 3; i++)
		for(j = 0; j < 3; j++)
			product.row[i][j] = v1[i] * v2[j];

	return product;
}
Matrix operator&(const Vector& v1, const Vector& v2)
{
	int N = v1.getn();
	if(N != v2.getn()){
		cerr << "cannot do outer product of " << N << " Vector with " <<
			v2.getn() << " Vector" << endl;
		exit(1);
	}

	int i, j;
	Matrix product(N, N);

	for(i = 0; i < N; i++)
		for(j = 0; j < N; j++)
			product.row[i][j] = v1[i] * v2[j];

	return product;
}
